package com.jmex.physics.impl.bullet;

import java.util.List;

import com.bulletphysics.linearmath.Transform;
import com.jme.math.Quaternion;
import com.jme.math.Vector3f;
import com.jmex.physics.DynamicPhysicsNode;
import com.jmex.physics.PhysicsNode;
import com.jmex.physics.impl.bullet.geometry.BulletGeometry;

public class BulletRigidBody {

	private PhysicsNode node;
	
	private Vector3f tempTrans = new Vector3f();
	private Vector3f tempPos = new Vector3f();
	private Quaternion tempRot = new Quaternion();
	
	private List<BulletGeometry> shapeList;
	
	private long nativeBulletHandle=-1;
	
	public BulletRigidBody(){}
	
	BulletRigidBody(PhysicsNode parent, List<BulletGeometry> collisionShapes)
	{
		node = parent;
		shapeList = collisionShapes;
	}
	
	public void setCollisionShapes(List<BulletGeometry> collisionShapes)
	{
		shapeList = collisionShapes;
	}
	
	public long getNativeBulletHandle() {
		return nativeBulletHandle;
	}

	public BulletGeometry[] getShapes()
	{
		return shapeList.toArray(new BulletGeometry[shapeList.size()]);
	}
	
	public void updateRigidBody()
	{
		if(node.isStatic())
			((BulletStaticPhysicsNode)node).getCenterOfMass(tempTrans);
		else
			((DynamicPhysicsNode)node).getCenterOfMass(tempTrans);
		nativeBulletHandle = BulletNativeInvoker.rebuildRigidBody(this,
				node.getWorldTranslation().x,node.getWorldTranslation().y,node.getWorldTranslation().z,
        		node.getWorldRotation().x,node.getWorldRotation().y,node.getWorldRotation().z,node.getWorldRotation().w,
        		node.getWorldScale().x,node.getWorldScale().y,node.getWorldScale().z,
        		tempTrans.x,tempTrans.y,tempTrans.z,
        		0,0,0,
        		0,0,0,
        		node.isStatic()?0:((DynamicPhysicsNode)node).getMass());
	}
		
	public void applyPhysicsMovement(double posX, double posY, double posZ, double rotX, double rotY, double rotZ, double rotW)
	{
//		System.out.println("pos="+posX+","+posY+","+posZ);
//		System.out.println("rot="+rotX+","+rotY+","+rotZ+","+rotW);
        tempRot.set((float)rotX,(float)rotY,(float)rotZ,(float)rotW);
        tempTrans.set((float)posX,(float)posY,(float)posZ);
        if ( node.getParent() != null ) {
	    	node.setLocalRotation(node.getParent().getWorldRotation().inverse().mult(tempRot));
	    	tempRot.mult(((DynamicPhysicsNode)node).getCenterOfMass(tempPos),tempPos);
            tempTrans.addLocal(tempPos.mult(-1));
        	tempTrans.subtractLocal(node.getParent().getWorldTranslation());
        	node.getParent().getWorldRotation().inverse().mult(tempTrans, node.getLocalTranslation());
        } else {
            node.getLocalRotation().set(tempRot);
        	node.getWorldRotation().mult(((DynamicPhysicsNode)node).getCenterOfMass(tempPos),tempPos);
            tempTrans.addLocal(tempPos.mult(-1));
        	node.getLocalTranslation().set( tempTrans );
        }
        node.updateWorldVectors();
        if(!node.isStatic())
        	((BulletDynamicPhysicsNode)node).sync();
	}

	public boolean isActive()
	{
		return node.isActive();
	}
}
